home *** CD-ROM | disk | FTP | other *** search
/ CD Actual 90 / CD Actual 90.iso / Software3D / HexaGreat-3D / 2d_3d.cpp next >
Encoding:
C/C++ Source or Header  |  1998-05-14  |  10.0 KB  |  413 lines
  1. /**********************************************************************
  2. Algorithm for getting 3-D coordinates 
  3. from 2-D coordinates in mirror symmetric model
  4.  
  5. Copyright (C) 1993 Shusaku Furushima.
  6.  
  7. Caution: This source code cannot be executed itself.  Please understand 
  8. the concepts of this algorithm and modify this source code to fit your 
  9. products.
  10.  
  11. This source code is copyrighted, but you can freely use all or a part of 
  12. source code for your products (commercial use also allowed).
  13. **********************************************************************/
  14.  
  15. #include <stdio.h>
  16. #include <math.h>
  17.  
  18. #define real double
  19.     /* max point of qr analysis */
  20. #define RQRMP 10
  21. #define RQRNP 30
  22.  
  23. /******** Rcoord ********/
  24. typedef struct {
  25.     real x;
  26.     real y;
  27.     real z;
  28. } Rcoord;
  29.  
  30. /******** 4 x 4 matrix **********/
  31. typedef real Rmat[4][4];
  32.  
  33. /********** mathematical functions ********/
  34. extern void Rqranalysis(int m, int n, real qa[][RQRNP],
  35.                     real qy[], real qz[]);
  36.  
  37. /******** general 4x4 matrix *********/
  38. class Rmat44{
  39.     void initmat(Rmat amat);
  40.   public:
  41.     void matmul(Rmat mat2);
  42.     Rmat mat;
  43.     void IRmat44();
  44.     void rotp(real p);
  45.     void rotq(real p);
  46.     void rotr(real p);
  47.     void copyfrom(Rmat mat2);
  48.     void move(real x, real y, real z);
  49.     void bepers();
  50.     void mul(real x, real y, real z);
  51.     void getcoord2(Rcoord *coord, Rcoord *cd, int pers);
  52. };
  53.  
  54. /********* 4x4 matrix for display objects *********/
  55. class Rmatobj{
  56.     public:
  57.     Rmat44 *devmat;            /* matrix of [3D -> 2D screen] */
  58.     Rmat44 *symm;            /* symmetrical matrix of devmat */
  59.     real pp, pq, pr;        /* rotate angle */
  60.     real dx, dy, dz;        /* move coordinate */
  61.     real bx, by;            /* scale to screen 2d coord */
  62.     int    pers;                /* flag if pers:1 or ortho:0 */
  63.  
  64.     void IRmatobj();                    /* init matrixes */
  65.     void makedev();                        /* make devmat */
  66.     void setsymmatx();                    /* set symm matrix of devmat */
  67.     Rcoord getdev(Rcoord *pt);            /* get screen coord */
  68.     Rcoord calc3d(Rcoord *pt1, Rcoord *pt2); /* get 3D coord from 2D coord */
  69. };
  70.  
  71.  
  72.  
  73. /********** least square method by QR (without pivoting) ***********/
  74. /********* qranalysis simple version ********/
  75. /*   m    number of unknown param.        */
  76. /*   n    number of data sets.            */
  77. /*   qa   data param matrix.              */
  78. /*   qy   data value vector.              */
  79. /*   qz   returning answer.               */
  80. /*  qrnp  define value of points number.  */
  81. /*  qrmp  define value of param number.   */
  82. /******************************************/
  83. void Rqranalysis(int m,int n,real qa[][RQRNP],
  84.                 real qy[],real qz[])
  85. {
  86.  int i,j,k;
  87.  static real a,qq[RQRMP][RQRNP],rq[RQRMP][RQRMP];
  88.  
  89.  for( k = 1; k <= m; k++)
  90.     {
  91.     a = 0;
  92.       for(i = 1; i <=n; i++)
  93.         a = a + qa[k][i] * qa[k][i];
  94.       rq[k][k] = sqrt(a);
  95.       for( i = 1; i <= n; i++)
  96.         qq[k][i] = qa[k][i] / rq[k][k];
  97.       for( j = k+1; j <= m; j++)
  98.         {
  99.         a = 0;
  100.           for( i = 1; i <= n; i++)
  101.             a = a + qq[k][i] * qa[j][i];
  102.         rq[j][k] = a;
  103.           for( i = 1; i <= n; i++)
  104.             qa[j][i] = qa[j][i] - qq[k][i] * rq[j][k];
  105.         }
  106.     }
  107.  
  108.   for( i = 1; i <= m; i++)
  109.     {
  110.     a = 0;
  111.       for( j = 1; j <= n; j++)
  112.         a = a + qq[i][j] * qy[j];
  113.     qz[i] = a;
  114.     }
  115.   qz[m] = qz[m] / rq[m][m];
  116.   for( i = m - 1; i >= 1; i--)
  117.     {
  118.       for( j = i + 1; j <= m; j++)
  119.         qz[i] = qz[i] - qz[j] * rq[j][i];
  120.     qz[i] = qz[i] / rq[i][i];
  121.     }
  122. }
  123.  
  124. /******** mathod for general matrix Rmat44 *********/
  125. /*** initialise matrix ***/
  126. void Rmat44::IRmat44()
  127. {
  128.     initmat(mat);
  129. }
  130.  
  131. /*** set [I] matrix ***/
  132. void Rmat44::initmat(Rmat amat)
  133. {
  134.     int i, j;
  135.     for (i = 0; i < 4; i++){
  136.         for (j = 0; j < 4; j++){
  137.             amat[i][j] = 0.0;
  138.             if (i == j)
  139.                 amat[i][j] = 1.0;
  140.         }
  141.     }
  142. }
  143.  
  144. /*** rotate around Y axis ***/
  145. void Rmat44::rotp(real p)
  146. {
  147.     Rmat tmp;
  148.     
  149.     initmat(tmp);
  150.     tmp[0][0] = cos(p);
  151.     tmp[2][0] = sin(p);
  152.     tmp[0][2] = - sin(p);
  153.     tmp[2][2] = cos(p);
  154.     matmul(tmp);
  155. }
  156.  
  157. /*** rotate around X axis ***/
  158. void Rmat44::rotq(real p)
  159. {
  160.     Rmat tmp;
  161.     
  162.     initmat(tmp);
  163.     tmp[1][1] = cos(p);
  164.     tmp[2][1] = -sin(p);
  165.     tmp[1][2] = sin(p);
  166.     tmp[2][2] = cos(p);
  167.     matmul(tmp);
  168. }
  169.  
  170. /*** rotate around Z axis ***/
  171. void Rmat44::rotr(real p)
  172. {
  173.     Rmat tmp;
  174.     
  175.     initmat(tmp);
  176.     tmp[0][0] = cos(p);
  177.     tmp[1][0] = -sin(p);
  178.     tmp[0][1] = sin(p);
  179.     tmp[1][1] = cos(p);
  180.     matmul(tmp);
  181. }
  182.  
  183. /*** shift coord ***/
  184. void Rmat44::move(real x, real y, real z)
  185. {
  186.     Rmat tmp;
  187.     
  188.     initmat(tmp);
  189.     tmp[0][3] = x;
  190.     tmp[1][3] = y;
  191.     tmp[2][3] = z;
  192.     matmul(tmp);
  193. }
  194.  
  195. /*** set to perspective matrix ***/
  196. void Rmat44::bepers()
  197. {
  198.     Rmat tmp;
  199.     
  200.     initmat(tmp);
  201.         /* project plane is always z=1 and eye point is x=y=z=0 */
  202.     tmp[3][2] = 1.0;
  203.     matmul(tmp);
  204. }
  205.  
  206. /*** magnify value ***/
  207. void Rmat44::mul(real x, real y, real z)
  208. {
  209.     Rmat tmp;
  210.     
  211.     initmat(tmp);
  212.     tmp[0][0] = x;
  213.     tmp[1][1] = y;
  214.     tmp[2][2] = z;
  215.     matmul(tmp);
  216. }
  217.  
  218. /*** multiple two matrices ***/
  219. void Rmat44::matmul(Rmat mat2)
  220. {
  221.     int i, j, k;
  222.     Rmat tmp;
  223.     
  224.     for (i = 0; i < 4; i++){
  225.         for (j = 0; j < 4; j++){
  226.             tmp[i][j] = mat[i][j];
  227.         }
  228.     }
  229.     for (i = 0; i < 4; i++){
  230.         for (j = 0; j < 4; j++){
  231.             mat[i][j] = 0.0;
  232.             for (k = 0; k < 4; k++){
  233.                 mat[i][j] += tmp[k][j] * mat2[i][k];
  234.             }
  235.         }
  236.     }
  237. }
  238.  
  239. /*** copy matrix ***/
  240. void Rmat44::copyfrom(Rmat mat2)
  241. {
  242.     int i,j;
  243.     for (i = 0; i < 4; i++){
  244.         for (j = 0; j < 4; j++){
  245.             mat[i][j] = mat2[i][j];
  246.         }
  247.     }
  248. }
  249.  
  250. /*** get 2D coordinates from 3D coordinates from the matrix ***/
  251. void Rmat44::getcoord2(Rcoord *coord, Rcoord *cd, int pers)
  252. {
  253.     int i,j;
  254.     real w;
  255.     
  256.     coord->x = mat[0][0] * cd->x + mat[0][1] * cd->y +
  257.                 mat[0][2] * cd->z + mat[0][3];
  258.     coord->y = mat[1][0] * cd->x + mat[1][1] * cd->y +
  259.                 mat[1][2] * cd->z + mat[1][3];
  260.     coord->z = 0.0;
  261.     if (pers){
  262.         w     = mat[3][0] * cd->x + mat[3][1] * cd->y +
  263.                 mat[3][2] * cd->z + mat[3][3];
  264.         w = 1.0 / w;
  265.         coord->x *= w;
  266.         coord->y *= w;
  267.     }
  268. }
  269.  
  270. /************* method for Roku matrix Rmatobj **************/
  271. /*** initialize matrices ***/
  272. void Rmatobj::IRmatobj()
  273. {
  274.     devmat = new(Rmat44);
  275.     symm = new(Rmat44);
  276.     devmat->IRmat44();
  277.     symm->IRmat44();
  278.                         /*** You can change these values in your application ***/
  279.     pp = -0.9;                /* default angle */
  280.     pq = -0.25;
  281.     pr = 0.0;
  282.     
  283.     dx = 0.0;                /* default position */        
  284.     dy = 0.0;
  285.     dz = 1.5;
  286.     
  287.     bx = by = 100.0;        /* default magnify value */
  288.     
  289.     pers = 1;                /* default is perspective (not orthogonal) */
  290. }
  291.  
  292. /*** make matrix of [3D coordinates -> 2D screen coordinates] ***/
  293. void Rmatobj::makedev()
  294. {
  295.     devmat->IRmat44();
  296.     devmat->rotp(pp);            /* rotate around Y axis */
  297.     devmat->rotq(pq);            /* rotate around X axis */
  298.     devmat->rotr(pr);            /* rotate around Z axis */
  299.     devmat->move(dx, dy, dz);    /* shift position */
  300.     if (pers)
  301.         devmat->bepers();        /* set to perspective matrix */
  302.     dsmat->mul(bx, by, 1.0);     /* magnify to be screen scale */
  303. }
  304.  
  305. /*** get 2D screen coordinates from 3D coordinates ***/
  306. Rcoord Rmatobj::getdev(Rcoord *pt)
  307. {
  308.     Rcoord tmp;
  309.  
  310.     devmat->getcoord2    (&tmp, pt, pers);
  311.     return(tmp);
  312. }
  313.  
  314. /*** set plane symmetry matrix ***/
  315. void Rmatobj::setsymmatx()
  316. {
  317.     symm->IRmat44();
  318.     symm->mat[0][0] = - 1.0;
  319.     symm->matmul(devmat->mat);
  320. }
  321.  
  322. /*** calc 3D coordinates from two 2D coordinates ***/
  323. Rcoord Rmatobj::calc3d(Rcoord *pt1, Rcoord *pt2)
  324. {
  325.     Rcoord p;
  326.     real qa[RQRMP][RQRNP], qy[RQRNP], qz[RQRMP];
  327.     
  328.     qa[1][1] = devmat->mat[0][0] - devmat->mat[3][0] * pt1->x;
  329.     qa[2][1] = devmat->mat[0][1] - devmat->mat[3][1] * pt1->x;
  330.     qa[3][1] = devmat->mat[0][2] - devmat->mat[3][2] * pt1->x;
  331.     qy[1] = - devmat->mat[0][3] + devmat->mat[3][3] * pt1->x;
  332.  
  333.     qa[1][2] = devmat->mat[1][0] - devmat->mat[3][0] * pt1->y;
  334.     qa[2][2] = devmat->mat[1][1] - devmat->mat[3][1] * pt1->y;
  335.     qa[3][2] = devmat->mat[1][2] - devmat->mat[3][2] * pt1->y;
  336.     qy[2] = - devmat->mat[1][3] + devmat->mat[3][3] * pt1->y;
  337.  
  338.     qa[1][3] = symm->mat[0][0] - symm->mat[3][0] * pt2->x;
  339.     qa[2][3] = symm->mat[0][1] - symm->mat[3][1] * pt2->x;
  340.     qa[3][3] = symm->mat[0][2] - symm->mat[3][2] * pt2->x;
  341.     qy[3] = - symm->mat[0][3] + symm->mat[3][3] * pt2->x;
  342.  
  343.     qa[1][4] = symm->mat[1][0] - symm->mat[3][0] * pt2->y;
  344.     qa[2][4] = symm->mat[1][1] - symm->mat[3][1] * pt2->y;
  345.     qa[3][4] = symm->mat[1][2] - symm->mat[3][2] * pt2->y;
  346.     qy[4] = - symm->mat[1][3] + symm->mat[3][3] * pt2->y;
  347.     
  348.     Rqranalysis(3, 4, qa, qy, qz);
  349.     p.x = qz[1];
  350.     p.y = qz[2];
  351.     p.z = qz[3];
  352.     return(p);
  353. }
  354.  
  355. /************** summary of 2D-3D conversion routine in HexaGreat-3D *****************/
  356. void main()
  357. {
  358.     Rmatobj *rmat;
  359.     
  360.     Rcoord gridscpt[MAX], gridpt[MAX];            /* Grid data */
  361.     int ngpoint, ngline, gridline[MAX][2];
  362.     
  363.     Rcoord inputscpt[MAX][2], inputpt[MAX][2];    /* Drawing data */
  364.     int ninputpt, ninputln, inputline[MAX][2];
  365.     
  366.     short x, y;
  367.     int i;
  368.     
  369.             :
  370.     // set grid coordinates and line informations.
  371.             :
  372.     rmat = new(Rmatobj);
  373.     rmat->IRmatobj();                    /* initialize matrix */
  374.             :
  375.     // changing display parameters (such as rmat->pp)
  376.             :
  377.     rmat->makedev();                    /* get conversion matrix */
  378.  
  379.     for (i = 0; i < ngpoint; i++){        /* calculate screen coordinates of grid */
  380.         gridscpt[i] = rmat->getdev(&gridpt[i]);
  381.     }
  382.     for (i = 0; i < ngline; i++){        /* display grid to screen */
  383.         x = (short)(gridscpt[ gridline[i][0] ].x);
  384.         y = (short)(gridscpt[ gridline[i][0] ].y);
  385.         MoveTo(x, y);
  386.         x = (short)(gridscpt[ gridline[i][1] ].x);
  387.         y = (short)(gridscpt[ gridline[i][1] ].y);
  388.         LineTo(x, y);
  389.     }
  390.             :
  391.     // input 2D drawings.
  392.     // at this moment, inputscpt[i][0] contains 2D screen coordinate of 
  393.     // left side point, and inputscpt[i][1] contains 2D screen coordinate 
  394.     // of right side point.
  395.     // (only x and y values are used. z value is not used.
  396.             :
  397.     rmat->setsymmatx();                    /* set symmetric matrix */
  398.     for (i = 0; i < ninputpt; i++){
  399.                                         /* calculate 3D coordinate */
  400.         inputpt[i][0] = rmat->calc3d(&inputscpt[i][0], &inputscpt[i][1]);
  401.                                         /* invert X axis sign */
  402.         inputpt[i][1] = inputpt[i][0];
  403.         inputpt[i][1].x = - inputpt[i][1].x;
  404.     }
  405.             :
  406.     // consequently, inputpt[i][0] contains 3D coordinate of left side point, 
  407.     // and inputpt[i][1] contains 3D coordinate of right side point.
  408.     
  409. }
  410.  
  411.  
  412.  
  413.